<<<<<<< .mine
#pragma config(Hubs,  S1, HTMotor,  HTServo,  none,     none)
#pragma config(Hubs,  S2, HTMotor,  HTMotor,  HTMotor,  none)
#pragma config(Sensor, S3,     ProtoBoard,     sensorI2CCustomFastSkipStates9V)
#pragma config(Sensor, S4,     IRMux,          sensorI2CCustom)
#pragma config(Motor,  mtr_S1_C1_1,     RightBack,     tmotorTetrix, PIDControl, encoder)
#pragma config(Motor,  mtr_S1_C1_2,     LeftBack,      tmotorTetrix, PIDControl, reversed, encoder)
#pragma config(Motor,  mtr_S2_C1_1,     RightFront,    tmotorTetrix, PIDControl, encoder)
#pragma config(Motor,  mtr_S2_C1_2,     LeftFront,     tmotorTetrix, PIDControl, reversed, encoder)
#pragma config(Motor,  mtr_S2_C2_1,     LeftLift,      tmotorTetrix, openLoop, encoder)
#pragma config(Motor,  mtr_S2_C2_2,     RightLift,     tmotorTetrix, openLoop, encoder)
#pragma config(Motor,  mtr_S2_C3_1,     TopLift,       tmotorTetrix, openLoop, encoder)
#pragma config(Motor,  mtr_S2_C3_2,     motorK,        tmotorTetrix, openLoop)
#pragma config(Servo,  srvo_S1_C2_1,    LeftGripper1,         tServoStandard)
#pragma config(Servo,  srvo_S1_C2_2,    LeftGripper2,         tServoStandard)
#pragma config(Servo,  srvo_S1_C2_3,    RightGripper1,        tServoStandard)
#pragma config(Servo,  srvo_S1_C2_4,    RightGripper2,        tServoStandard)
#pragma config(Servo,  srvo_S1_C2_5,    LateralGripper,       tServoContinuousRotation)
#pragma config(Servo,  srvo_S1_C2_6,    servo6,               tServoNone)
//*!!Code automatically generated by 'ROBOTC' configuration wizard               !!*//

#include "JoystickDriver.c"  //Include file to "handle" the Bluetooth messages.

#include "LiftManager.h"						//Automatic lift positioner
#include "3785_motion_functions.h"	//Motion fuctions such as forwards and backwards

void gripperAdjust(int position)  //Sets all gripper servos to mirrored positions with one function
{
	servo[LeftGripper1]=position;
	servo[LeftGripper2]=position;
	servo[RightGripper1]=230-position;
	servo[RightGripper2]=230-position;
}

void initializeRobot()  //Initalizes servos before autonomous starts
{
	//gripperAdjust(125);		//Closes the gripper to hold the autonomous ring
	return;
}

task main()
{

	initializeRobot();  //Initalizes the servos (closes the gripper to hold the autonomous ring
	waitForStart(); //Wait for the beginning of autonomous phase

	//goToPosition(150); //Raises the lift to the height of the dispenser so that we are ready to go in teleop

	straight(50,72); //moves forward to the other side of the field
	pause();  //waits half a second to allow the robot to come to a full stop and to reduce the stress on the motors

	turnRight(50,1300);  //Turns the robot 90 degrees to face the peg rack
	pause();  //waits half a second to allow the robot to come to a full stop and to reduce the stress on the motors

	straight(50,5); //moves forward to the edge of the peg rack
	pause();  //waits half a second to allow the robot to come to a full stop and to reduce the stress on the motors

	while(true)	//runs for the rest of autonomous to continually block other robots
	{
		straight(50,40);	//moves forward the length of the pegboard
		pause();  //waits half a second to allow the robot to come to a full stop and to reduce the stress on the motors
		straight(-50,40);	//moves backwards the length of the pegboard
	  pause();  //waits half a second to allow the robot to come to a full stop and to reduce the stress on the motors
	}
}
=======
#pragma config(Hubs,  S1, HTMotor,  HTServo,  none,     none)
#pragma config(Hubs,  S2, HTMotor,  HTMotor,  HTMotor,  none)
#pragma config(Sensor, S3,     ProtoBoard,     sensorI2CCustomFastSkipStates9V)
#pragma config(Sensor, S4,     IRMux,          sensorI2CCustom)
#pragma config(Motor,  mtr_S1_C1_1,     RightBack,     tmotorTetrix, PIDControl, encoder)
#pragma config(Motor,  mtr_S1_C1_2,     LeftBack,      tmotorTetrix, PIDControl, reversed, encoder)
#pragma config(Motor,  mtr_S2_C1_1,     RightFront,    tmotorTetrix, PIDControl, encoder)
#pragma config(Motor,  mtr_S2_C1_2,     LeftFront,     tmotorTetrix, PIDControl, reversed, encoder)
#pragma config(Motor,  mtr_S2_C2_1,     LeftLift,      tmotorTetrix, openLoop, encoder)
#pragma config(Motor,  mtr_S2_C2_2,     RightLift,     tmotorTetrix, openLoop, encoder)
#pragma config(Motor,  mtr_S2_C3_1,     TopLift,       tmotorTetrix, openLoop, encoder)
#pragma config(Motor,  mtr_S2_C3_2,     motorK,        tmotorTetrix, openLoop)
#pragma config(Servo,  srvo_S1_C2_1,    LeftGripper1,         tServoStandard)
#pragma config(Servo,  srvo_S1_C2_2,    LeftGripper2,         tServoStandard)
#pragma config(Servo,  srvo_S1_C2_3,    RightGripper1,        tServoStandard)
#pragma config(Servo,  srvo_S1_C2_4,    RightGripper2,        tServoStandard)
#pragma config(Servo,  srvo_S1_C2_5,    LateralGripper,       tServoContinuousRotation)
#pragma config(Servo,  srvo_S1_C2_6,    servo6,               tServoNone)
//*!!Code automatically generated by 'ROBOTC' configuration wizard               !!*//

#include "JoystickDriver.c"  //Include file to "handle" the Bluetooth messages.

#include "LiftManager.h"						//Automatic lift positioner
#include "3785_motion_functions.h"	//Motion fuctions such as forwards and backwards

void gripperAdjust(int position)  //Sets all gripper servos to mirrored positions with one function
{
	servo[LeftGripper1]=position;
	servo[LeftGripper2]=position;
	servo[RightGripper1]=255-position;
	servo[RightGripper2]=255-position;
}

void initializeRobot()  //Initalizes servos before autonomous starts
{
	gripperAdjust(125);		//Closes the gripper to hold the autonomous ring
	return;
}

task main()
{

	initializeRobot();  //Initalizes the servos (closes the gripper to hold the autonomous ring
	waitForStart(); //Wait for the beginning of autonomous phase

	goToPosition(150); //Raises the lift to the height of the dispenser so that we are ready to go in teleop

	straight(50,20); //moves forward to the other side of the field
	pause();  //waits half a second to allow the robot to come to a full stop and to reduce the stress on the motors

	turnRight(50,2000);  //Turns the robot 90 degrees to face the peg rack
	pause();  //waits half a second to allow the robot to come to a full stop and to reduce the stress on the motors

	straight(50,20); //moves forward to the edge of the peg rack
	pause();  //waits half a second to allow the robot to come to a full stop and to reduce the stress on the motors

	while(true)	//runs for the rest of autonomous to continually block other robots
	{
		straight(50,50);	//moves forward the length of the pegboard
		pause();  //waits half a second to allow the robot to come to a full stop and to reduce the stress on the motors
		straight(-50,50);	//moves backwards the length of the pegboard
	  pause();  //waits half a second to allow the robot to come to a full stop and to reduce the stress on the motors
	}
}
>>>>>>> .r68
